import sensor, image, pyb
import time
from pyb import Servo,Pin
value1 = 0 			#读入p_in引脚的值
value2 = 0 			#读入p_in引脚的值
value3 = 0 			#读入p_in引脚的值
value4 = 0
p1 = Pin('P1', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻
p2 = Pin('P2', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻
p3 = Pin('P3', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻
p4 = Pin('P4', Pin.IN, Pin.PULL_UP)#设置p_in为输入引脚，并开启上拉电阻
def read_pins():
    global value1,value2,value3,value4
    value1 = p1.value() 			#读入p_in引脚的值
    value2 = p2.value() 			#读入p_in引脚的值
    value3 = p3.value() 			#读入p_in引脚的值
    value4 = p4.value()
    #print("value1",value1)
    #print("value2",value2)
    #print("value3",value3)
    #print("value4",value4)
def servo_init():
    servo_x.angle(50)  # 设置舵机的初始位置40 50 60
    servo_y.angle(58)
def move_servo(servo_x, servo_y, start_angle_X, end_angle_X, start_angle_Y, end_angle_Y, steps, delay):
    angle_step_X = (end_angle_X - start_angle_X) / steps
    angle_step_Y = (end_angle_Y - start_angle_Y) / steps
    angle_X = start_angle_X
    angle_Y = start_angle_Y
    for _ in range(steps + 1):
        servo_x.angle(angle_X)
        servo_y.angle(angle_Y)
        time.sleep_ms(delay)
        angle_X += angle_step_X
        angle_Y += angle_step_Y

read_pins()
def move_juxing():
    start_x = 38.5
    end_x = 58.7
    start_y = 46
    end_y = 65
    steps = 1000
    delay = 1

    move_servo(servo_x, servo_y, 50, start_x, 58, start_y, steps, delay)
    move_servo(servo_x, servo_y, start_x, end_x, start_y, start_y, steps, delay)
    move_servo(servo_x, servo_y, end_x, end_x, start_y, end_y, steps, delay)
    move_servo(servo_x, servo_y, end_x, start_x, end_y, end_y, steps, delay)
    move_servo(servo_x, servo_y, start_x, start_x, end_y, start_y, steps, delay)
    time.sleep(0.5)
    move_servo(servo_x, servo_y, start_x, 50, start_y, 58, steps, delay)
    time.sleep(0.5)



# 设置角度范围（假设舵机角度范围为0到180度）
angle_min = 0
angle_max = 180
# 定义PID控制器
class PIDController:
    def __init__(self, Kp, Ki, Kd):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.prev_error = 0
        self.integral = 0

    def compute(self, setpoint, feedback):
        error = setpoint - feedback
        self.integral += error
        derivative = error - self.prev_error
        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        self.prev_error = error
        return output

# 创建PID控制器对象
pid_x = PIDController(-0.26, -0.001, -0.022)
pid_y = PIDController(-0.3, -0.001, -0.022)
# 定义定时器回调函数
steps = 1000
current_point_index = 0 #当前角点位置索引
current_substep = 0  # 当前细分位置
last_output_x=0
last_output_y=0

def timer_callback(timer):
            if value3==0:
                global current_point_index, current_substep, corner_points, steps,last_output_y,last_output_x
                steps = 90
                start_x, start_y = corner_points[current_point_index]
                end_x, end_y = corner_points[(current_point_index + 1) % 4]
                step_x = (end_x - start_x) / steps
                step_y = (end_y - start_y) / steps
                target_x = start_x +current_substep*step_x
                target_y = start_y + current_substep*step_y
                # 使用PID控制计算输出
                if(abs(target_x - 55) <= 2 and abs(target_y - 42) <= 2):
                   current_substep += 3
                if current_substep >= steps and abs(target_x - 55) <= 2 and abs(target_y - 42) <= 2:
            # 更新 current_point_index，并重置 current_substep
                   current_point_index += 1
                   current_substep=0
                if current_point_index >= 4:
                   current_point_index = 0
                   current_substep = 0

                output_x = 50+pid_x.compute(target_x, 55)
                output_y = 58+pid_y.compute(target_y, 42)
                #if last_output_x-output_x>1 and last_output_x !=0:
                     #output_x=output_x*0.9
                #if last_output_y-output_y>1 and last_output_y !=0:
                     #output_y=output_y*0.9
                #last_output_x=output_x
                #last_output_y=output_y
                #print("output_x",output_x)
                #print("output_y",output_y)

                # 限制输出在舵机角度范围内
                #output_x = max(angle_min, angle_max)
                #output_y = max(angle_min, angle_max)
                #output_x = last_output_x + (output_x - last_output_x) * 0.1;
                #output_y = last_output_y + (output_y - last_output_y) * 0.1;
                #last_output_x = output_x;
                #last_output_y = output_y;

                servo_x.angle(output_x)
                servo_y.angle(output_y)


# 创建舵机对象，参数为舵机的接口编号



sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE)
sensor.set_framesize(sensor.QQVGA)
sensor.set_hmirror(True)  # 左右翻转
sensor.set_vflip(True)   # 上下翻转
sensor.set_windowing((90,80))
sensor.set_brightness(1)   #设置亮度
sensor.set_contrast(-1) #对比度
#sensor.set_gainceiling(2)   #增益上限
sensor.set_auto_gain(True,gain_db=7) #增益

clock = time.clock()
servo_x = Servo(1)
servo_y = Servo(2)
servo_x.angle(48)  # 设置舵机的初始位置40 50 60
servo_y.angle(55)
move_servo(servo_x, servo_y, 48, 50, 55, 58, 1000, 1)
# 保存角点坐标的列表
corner_points = []
stable_rect_found = 0
if value1==1:
    move_juxing()
#time.sleep(0.5)
# 设置定时器，设置频率和回调函数


while True:
    clock.tick()
    read_pins()
    if value2==1:
        img = sensor.snapshot()
        if not stable_rect_found:  # only search for rects if stable rectangle has not been found
            rects = img.find_rects(threshold=25000)
        if rects :

            # 获取矩形的四个角点
                corners = rects[0].corners()
                c1 = (corners[0][0]+1,corners[0][1]-1)
                c4 = (corners[1][0]-1,corners[1][1]-1)
                c3 = (corners[2][0]-1,corners[2][1]+1)
                c2 = (corners[3][0]+1,corners[3][1]+1)

             #绘制四个角点
                #img.draw_cross(c1[0], c1[1], color=(255, 255, 255))
                #img.draw_cross(c2[0], c2[1], color=(255, 0, 255))
                #img.draw_cross(c3[0], c3[1], color=(255, 255, 255))
                #img.draw_cross(c4[0], c4[1], color=(255, 255, 255))

                ## 绘制连线并保存角点坐标
                #img.draw_line(c1[0], c1[1], c2[0], c2[1], color=(0, 255, 255))
                #img.draw_line(c2[0], c2[1], c3[0], c3[1], color=(0, 255, 255))
                #img.draw_line(c3[0], c3[1], c4[0], c4[1], color=(0, 255, 255))
                #img.draw_line(c4[0], c4[1], c1[0], c1[1], color=(0, 255, 255))

                # 将四个角点添加到角点列表中

                corner_points = [c1,c2,c3,c4]
                #stable_rect_found = True  # set flag to indicate a stable rectangle has been found
                timer = pyb.Timer(2, freq=800) # 设置定时器，频率根据步数计算
                timer.callback(timer_callback)  # 设置定时器回调函数


